from matplotlib import pyplot as plt
import pandas as pd
data = pd.read_csv('../validation.csv')
t, pre_x, pre_y, lab_x, lab_y = data['tmsp'],data['predict_x'],data['predict_y'],data['groundtruth_x'],data['groundtruth_y']
plt.plot(pre_x,pre_y,label='Predict',color='blue',linewidth=3)
plt.plot(lab_x, lab_y,label='Groundtruth',color='red',linewidth=3)
print(lab_x.shape, lab_y.shape)
plt.scatter(lab_x[0], lab_y[0], color='green', s=100, label='Start')  # 起点标注为绿色
plt.scatter(lab_x[len(lab_x)-1], lab_y[len(lab_y)-1], color='red', s=100, label='End')  # 终点标注为红色

# 添加注释文本到起点和终点
plt.annotate('Start', (lab_x[0], lab_y[0]), textcoords="offset points", xytext=(10,-10), ha='center')
plt.annotate('End', (lab_x[len(lab_x)-1], lab_y[len(lab_y)-1]), textcoords="offset points", xytext=(10,-10), ha='center')

plt.title('ENU Path')
plt.xlabel('East (meters)')
plt.ylabel('North (meters)')
plt.grid()
plt.axis('equal')
plt.show()

import math
import numpy as np


# 将x, y转换为极坐标
def cartesian_to_polar(x1, y1, x2, y2):
    # 计算相对距离 r
    r = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)

    # 计算相对角度 theta (使用 atan2)
    theta = math.atan2(y2 - y1, x2 - x1)

    # 返回 r (距离) 和 theta (弧度)
    return r, theta


# 示例：两个点的 x, y 坐标
x1, y1 = -0.518781811,	0.815354693  # 上一个点
x2, y2 = -0.570952339,	0.897436199 # 当前点


# 计算极坐标
r, theta = cartesian_to_polar(x1, y1, x2, y2)

# 输出结果
print(f"相对距离 r = {r:.2f} 米")
print(f"相对角度 θ = {theta:.2f} 弧度 ({math.degrees(theta):.2f} 度)")

#plt.savefig('img.png')